import jason.asSemantics.DefaultInternalAction;
import jason.asSemantics.TransitionSystem;
import jason.asSemantics.Unifier;
import jason.asSyntax.Atom;
import jason.asSyntax.NumberTerm;
import jason.asSyntax.Term;

import java.awt.Point;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;
import java.util.TreeSet;
import java.util.logging.Level;

import prop.PropertiesReader;

import arch.CowboyArch;
import arch.LocalWorldModel;
import env.WorldModel;

public class astar extends DefaultInternalAction {
        
        public LocalWorldModel model = null;
        
    Node[][] nodes = null;
    
        public astar() {
        }
        
    @Override
    public Object execute(TransitionSystem ts, Unifier un, Term[] args) throws Exception {
        try { 
                long start = System.currentTimeMillis();
            int myX = (int)((NumberTerm)args[0]).solve();
                        int myY = (int)((NumberTerm)args[1]).solve();
                        int targetX = (int)((NumberTerm)args[2]).solve();
                        int targetY = (int)((NumberTerm)args[3]).solve();
                        
                        /*
                        //If dist is large, dont use AStar since it is costly
                        if(Math.abs(myX - targetX) + Math.abs(myY - targetY) > 30){                             
                                return un.unifies(args[4], new Atom("skip"));
                        }
                        */
                        
                        
                        Solution sol = null;
                        
                        if(args.length == 6)
                        {
                                sol = AStarSearch.shortestPath(myX, myY, targetX, targetY, ts, false);
                        }
                        else
                        {
                                sol = AStarSearch.search(myX, myY, targetX, targetY, ts, false);
                        }
                        String action = "skip";
                        
                        if(sol.succeded()){
                                action = sol.getFirstMove();
                        }                       
                        long end = System.currentTimeMillis();
                        delegate_target.t1 += end-start;
                        return un.unifies(args[4], new Atom(action));
            
        } catch (Throwable e) {
            ts.getLogger().log(Level.SEVERE, "astar error: "+e, e);
        }
        return false;
    } // execute
    
    public static ArrayList<Node> getShortestPath(int startX, int startY, int endX, int endY,
                TransitionSystem ts, boolean fenceClear){
        //long start = System.currentTimeMillis();
        ArrayList<Node> temp = AStarSearch.shortestPath(startX, startY, endX, endY, ts, fenceClear).getPath();
                //long end = System.currentTimeMillis();
                //delegate_target.t1 += end-start;
        return temp;
    }
    
    //Returns the true shortest path without punishments
    public static Solution getShortestPath2(int startX, int startY, int endX, int endY,
                TransitionSystem ts, boolean fenceClear){
        //long start = System.currentTimeMillis();
        Solution temp = AStarSearch.trueShortestPath(startX, startY, endX, endY, ts, fenceClear);   
                //long end = System.currentTimeMillis();
                //delegate_target.t1 += end-start;
        return temp;
    }
}

class AStarSearch {
        
        //Returns shortest path with punishment for herders
        public static Solution search(int startX, int startY,
                        int endX, int endY, TransitionSystem ts, boolean fenceClear){
                
        CowboyArch arch = (CowboyArch)ts.getUserAgArch();
        LocalWorldModel model = arch.getModel();
                
        Heuristic h = new Heuristic(model, endX, endY);
        
                HashSet<Node> closed = new HashSet<Node>();
                PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);

                
                Node initialNode = new Node(startX, startY, null, 0, h);
                fringe.add(initialNode);

                while(true){    
                        if(fringe.isEmpty()){
                                return new Solution();
                        }
                        Node n = fringe.poll();
                        if(n.getX() == endX && n.getY() == endY)
                                return new Solution(initialNode, n);
                        
                        if(!(closed.contains(n))){
                                closed.add(n);
                                List<Node> children = n.expand(model, Node.PUNISHMENT, fenceClear);
                                fringe.addAll(children);
                        }                       
                }
        }
        
        //Returns the shortest path with special punishment for attackers
        public static Solution shortestPath(int startX, int startY,
                        int endX, int endY, TransitionSystem ts, boolean fenceClear){
                
        CowboyArch arch = (CowboyArch)ts.getUserAgArch();
        LocalWorldModel model = arch.getModel();
                
        Heuristic h = new Heuristic(model, endX, endY);
        
                HashSet<Node> closed = new HashSet<Node>();
                PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);

                
                Node initialNode = new Node(startX, startY, null, 0, h);
                fringe.add(initialNode);

                while(true){    
                        if(fringe.isEmpty()){
                                return new Solution();
                        }
                        Node n = fringe.poll();
                        if(n.getX() == endX && n.getY() == endY)
                                return new Solution(initialNode, n);
                        
                        if(!(closed.contains(n))){
                                closed.add(n);
                                List<Node> children = n.expand(model, Node.PUNISHMENT_SPECIAL, fenceClear);
                                fringe.addAll(children);
                        }                       
                }               
        }
        
        //Returns the true shortest path
        public static Solution trueShortestPath(int startX, int startY,
                        int endX, int endY, TransitionSystem ts, boolean fenceClear){
                
        CowboyArch arch = (CowboyArch)ts.getUserAgArch();
        LocalWorldModel model = arch.getModel();
                
        Heuristic h = new Heuristic(model, endX, endY);
        
                HashSet<Node> closed = new HashSet<Node>();
                PriorityQueue<Node> fringe = new PriorityQueue<Node>(50,h);

                
                Node initialNode = new Node(startX, startY, null, 0, h);
                fringe.add(initialNode);

                while(true){    
                        if(fringe.isEmpty()){
                                return new Solution();
                        }
                        Node n = fringe.poll();
                        if(n.getX() == endX && n.getY() == endY)
                                return new Solution(initialNode, n);
                        
                        if(!(closed.contains(n))){
                                closed.add(n);
                                List<Node> children = n.expand(model, Node.NO_PUNISHMENT, fenceClear);
                                fringe.addAll(children);
                        }                       
                }               
        }
} //AStarSearch

class Node{
        
        private int x,y;
        private Node parent;
        private Heuristic h;
        private int g;
        private int f;
        
        public Node(int x, int y, Node p, int g, Heuristic h){
                this.parent = p;
                this.g = g;
                this.h = h;
                this.x = x;
                this.y = y;
                f = g + h.heuristic(this);
        }
        
        public Node getParent(){
                return parent;
        }
        
        public static final int PUNISHMENT = 0;
        public static final int PUNISHMENT_SPECIAL = 1;
        public static final int NO_PUNISHMENT = 2;
        
        public List<Node> expand(LocalWorldModel model, int punishment, boolean fenceClear){
                List<Node> children = new ArrayList<Node>();            
                for(int i = -1; i <= 1; i++)
                        for(int j = -1; j <= 1; j++)
                                if(!(i == 0 && j == 0) &&
                                                i+x >= 0 && j+y >= 0 &&
                                                i+x < model.getWidth() && j+y < model.getHeight() &&
                                                isClear(x+i,y+j,model,fenceClear)){             
                                        int newG = punishment(model,x+i,y+j);
                                        int newGSpecial = punishmentSpecial(model,x+i,y+j);
                                        Node child;
                                        if(punishment == PUNISHMENT)
                                                child = new Node(i+x,j+y,this,this.g + newG + 1,h);
                                        else if(punishment == PUNISHMENT_SPECIAL)
                                                child = new Node(i+x,j+y,this,this.g + newGSpecial + 1,h);
                                        else
                                                child = new Node(i+x,j+y,this,this.g + 1,h);                                            
                                        children.add(child);
                                }
                return children;
    }

        private int punishment(LocalWorldModel model, int x, int y){
                int p = 0;
                
                Point cor = new Point(model.getCorralCenter().x, model.getCorralCenter().y);
                int xDir = x - cor.x;
                int yDir = y - cor.y;
                
                
                for(int i = -5; i <= 5; i++){
                        for(int j = -5; j <= 5; j++){
                                if(i+x >= 0 && i+x < model.getWidth() &&
                                        j+y >= 0 && j+y < model.getHeight()){
                                        if(model.hasObject(WorldModel.COW, x+i, y+j)){
                                                //If cow is not between (x,y) and corral
                                                if(xDir*i > 0 || yDir*j > 0)
                                                        p += 6 - Math.abs(i); //Punish harder if it gets closer to cow                                          
                                        }
                                }
                        }                       
                }
                
                if(model.hasObject(WorldModel.AGENT, x, y))
                        p += 5;
                
                return p;
        }
        
        private int punishmentSpecial(LocalWorldModel model, int x, int y){
                int p = 0;
        
                
                if(model.hasObject(WorldModel.FENCE, x, y))
                        p += 5;
                
                if(model.hasObject(WorldModel.AGENT, x, y))
                        p += 5;         
                if(model.hasObject(WorldModel.CORRAL, x, y))
                        p += 10;
                
                
                return p;
        }
        
    public String getMove(Node from, Node to) {
        if (to.x < from.x && to.y == from.y)
            return "west";
        else if (to.x > from.x && to.y == from.y)
            return "east";
        else if (to.x == from.x && to.y < from.y)
            return "north";
        else if (to.x > from.x && to.y < from.y)
            return "northeast";
        else if (to.x < from.x && to.y < from.y)
            return "northwest";
        else if (to.x == from.x && to.y > from.y)
            return "south";
        else if (to.x > from.x && to.y > from.y)
            return "southeast";
        else if (to.x < from.x && to.y > from.y)
            return "southwest";
        else
            return "skip";
    }
        
    private int getContent(int x, int y, LocalWorldModel model) {
        if (model.hasObject(WorldModel.CLEAN, x, y))
                return WorldModel.CLEAN;
        else if (model.hasObject(WorldModel.AGENT, x, y))
                return WorldModel.AGENT;
        else if (model.hasObject(WorldModel.OBSTACLE, x, y))
                return WorldModel.OBSTACLE;
        else if (model.hasObject(WorldModel.COW, x, y))
                return WorldModel.COW;
        else if (model.hasObject(WorldModel.CORRAL, x, y))
                return WorldModel.CORRAL;
        else if (model.hasObject(WorldModel.ENEMY, x, y))
                return WorldModel.ENEMY;
        else if (model.hasObject(WorldModel.ENEMYCORRAL, x, y))
                return WorldModel.ENEMYCORRAL;
        else if (model.hasObject(WorldModel.FENCE, x, y))
                return WorldModel.FENCE;
        else if (model.hasObject(WorldModel.SWITCH, x, y))
                return WorldModel.SWITCH;
        
        return 0;
    }
    
    public boolean isClear(int x, int y, LocalWorldModel model, boolean fenceClear) {
        int content = getContent(x,y,model);
        return content != WorldModel.OBSTACLE &&
                (content != WorldModel.FENCE || fenceClear) &&
                content != WorldModel.SWITCH &&
                content != WorldModel.ENEMY;
    }
    
        
        public int getG(){
                return g;
        }
        
        public int hashCode(){
                return (int)Math.pow(2, x) * (int)Math.pow(3, y);
        }
        
        public int getF(){
                return f;
        }
        
        public int getX(){
                return x;
        }
        
        public int getY(){
                return y;
        }
        
        public Heuristic getH(){
                return h;
        }

        
        public boolean equals(Object o){
                if(!(o instanceof Node))
                        return false;
                Node n = (Node) o;
                return n.x == this.x && n.y == this.y;
        }
        
} //Node

class Solution {

        private boolean succes;
        private ArrayList<Node> path;
        
        public Solution(){
                path = new ArrayList<Node>();
                succes = false;
        }
        
        public Solution(Node initial, Node terminal){
                succes = true;
                
                path = new ArrayList<Node>();
                
                path.add(terminal);
                Node n = terminal;

                while(!n.equals(initial)){
                        n = n.getParent();
                        path.add(0, n);
                }       
        }

        public boolean succeded(){
                return succes;
        }
        
        public ArrayList<Node> getPath(){
                return path;
        }
        
        public int getPathLength(){
                return path.size()-1;
        }

    public String getFirstMove() {
        if(!succes)
                return "skip";
        int xFrom = path.get(0).getX();
        int yFrom = path.get(0).getY();
        int xTo = path.get(1).getX();
        int yTo = path.get(1).getY();
        
        if (xTo < xFrom && yTo == yFrom)
            return "west";
        else if (xTo > xFrom && yTo == yFrom)
            return "east";
        else if (xTo == xFrom && yTo < yFrom)
            return "north";
        else if (xTo > xFrom && yTo < yFrom)
            return "northeast";
        else if (xTo < xFrom && yTo < yFrom)
            return "northwest";
        else if (xTo == xFrom && yTo > yFrom)
            return "south";
        else if (xTo > xFrom && yTo > yFrom)
            return "southeast";
        else if (xTo < xFrom && yTo > yFrom)
            return "southwest";
        else
            return "skip";
    }
}

class Heuristic implements Comparator<Node>{

        private LocalWorldModel model;
        private int endX, endY;
        
        public Heuristic(LocalWorldModel model, int endX, int endY){
                this.model = model;
                this.endX = endX;
                this.endY = endY;
        }
        
        public int heuristic(Node n) {
                return Math.max(Math.abs(n.getX() - endX), Math.abs(n.getY() - endY));
        }

        @Override
        public int compare(Node n0, Node n1) {
                if(n0.getF() < n1.getF()) return -1;
                else if(n0.getF() > n1.getF()) return 1;
                return -1;
        }                               
};